
#include "stm32f10x.h"
#include "I2C_Hard.h"
#include "Mpu6050_I2C_HARD.h"
#include "MPU6050_Reg.h"
#include "Delay.h"
#include <math.h>
#define MPU6050_6_AXISES_DATA_LENGTH 14

const static uint8_t slave_address = 0xd0;

/** 因为通过上面配置的低通滤波器和分频系数
    最终 采样率为 200Hz 
    所以采样周期为 5ms = 0.005s
*/ 

const static float dt = 0.005;
/**
因为陀螺仪的积分需要访问上一次的求和结果
所以这里声明一个全局变量
*/
static Euler_Angles  euler_angles;

static mpu6050_data  axises_data;

void init_dma_for_reading_6_axises_data(uint8_t * data){
    
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1,ENABLE );
    
    DMA_InitTypeDef DMA_InitStruct;
    DMA_StructInit(&DMA_InitStruct);
    
    DMA_InitStruct.DMA_BufferSize= MPU6050_6_AXISES_DATA_LENGTH; // 本次要传输多少个单位数据
    DMA_InitStruct.DMA_DIR=DMA_DIR_PeripheralSRC;
    DMA_InitStruct.DMA_M2M=DMA_M2M_Disable;
    DMA_InitStruct.DMA_Mode= DMA_Mode_Circular; //DMA_Mode_Normal;//
    DMA_InitStruct.DMA_Priority=DMA_Priority_Medium;
    
    DMA_InitStruct.DMA_MemoryBaseAddr= (uint32_t) data;
    DMA_InitStruct.DMA_MemoryDataSize=DMA_MemoryDataSize_Byte;
    DMA_InitStruct.DMA_MemoryInc=DMA_MemoryInc_Enable;
    
    
    DMA_InitStruct.DMA_PeripheralBaseAddr= I2C1->DR;
    DMA_InitStruct.DMA_PeripheralDataSize=DMA_PeripheralDataSize_Byte;
    DMA_InitStruct.DMA_PeripheralInc=DMA_PeripheralInc_Disable;
    
    
    
    DMA_Init(DMA1_Channel7,&DMA_InitStruct);
    
   
}

void write_command_data(uint8_t command,uint8_t data){

    send_n_byte(slave_address,(uint8_t[]){command,data},2);
}

void init_mpu_6050(void){

    
	 // 电源管理1
	 // 重置（复位）所有寄存器 
    write_command_data(MPU6050_PWR_MGMT_1,  0x80);
	
	    // 等待复位完成
    Delay_ms(100);

    // 关闭睡眠（唤醒）、使用陀螺仪的时钟
    write_command_data(MPU6050_PWR_MGMT_1,0x01);
    
    //电源管理寄存器2，保持默认值0，所有轴均不待机
    write_command_data(MPU6050_PWR_MGMT_2, 0x00);
    

    
    // 数字低通滤波器配置为6
    // 一旦配置了 低通滤波器，那么采样率就只能为 1kHz 而不是 8kHz
    write_command_data(MPU6050_CONFIG,0x06);
    
    // 配置采样率分频系数
    // 那么实际的采样率 = 1kHz/(1+4) = 200Hz
    write_command_data(MPU6050_SMPLRT_DIV,0x04);
    
    // 配置加速度量程为+- 2g
    // 并进行加速度计自检
    write_command_data(MPU6050_ACCEL_CONFIG,0x00);

    // 配置角速度传感器的量程为 +- 250°/s
    // 并进行陀螺仪三轴自检
    write_command_data(MPU6050_GYRO_CONFIG,0x00);
}




void read_n_bytes(uint8_t slave_register_address,uint8_t * data,uint8_t data_length){

    send_slave_address(slave_address);
    
    // 发送要读取的寄存器地址
    // 发送从机寄存器地址
    I2C_SendData(I2C1,slave_register_address);
    
    // Ev8_2
    // 等待 数据（这里是从机的寄存器地址）已经被转运到 移位寄存器
    wait_event(I2C_EVENT_MASTER_BYTE_TRANSMITTED);

 
    
    I2C_GenerateSTART(I2C1,ENABLE);
    
    wait_event(I2C_EVENT_MASTER_MODE_SELECT);
    
    
    
        // 发送从机地址
    I2C_Send7bitAddress(I2C1,slave_address,I2C_Direction_Receiver);
    
    // Ev6
    // 等待从机接收到自己的地址后，向主机发送一个 ack
    wait_event(I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED);
    
    for(uint8_t i=0;i<data_length;i++){
        
        if(i==data_length-1){
        
            I2C_AcknowledgeConfig(I2C1,DISABLE);
        
        }
        wait_event(I2C_EVENT_MASTER_BYTE_RECEIVED); 
        data[i] = I2C_ReceiveData(I2C1);
        
    }
    
    I2C_GenerateSTOP(I2C1,ENABLE);   
  
    I2C_AcknowledgeConfig(I2C1,ENABLE);
}

int16_t bytes2int16(uint8_t * bytes){
    int16_t result = bytes[0];
    result<<=8;
    result|=bytes[1];
    return result;
}

int16_t read_2_bytes_2_int16(uint8_t slave_register_address){

    uint8_t temp[2];
    
    read_n_bytes(slave_register_address,temp,2);


    return bytes2int16(temp);
}

mpu6050_data* read_6_axises( void){
    uint8_t temp[MPU6050_6_AXISES_DATA_LENGTH];
    
    read_n_bytes(MPU6050_ACCEL_XOUT_H,temp,MPU6050_6_AXISES_DATA_LENGTH);
    
    axises_data.x_acc = bytes2int16(temp);
    axises_data.y_acc = bytes2int16(temp+2);
    axises_data.z_acc = bytes2int16(temp+4);
    axises_data.tmeperature = bytes2int16(temp+6);
    axises_data.x_gyr = bytes2int16(temp+8);
    axises_data.y_gyr = bytes2int16(temp+10);
    axises_data.z_gyr = bytes2int16(temp+12);
    
    return &axises_data;
}


/**
 * 这样做能把 mpu6050的六轴数据解析为物理上标准的 m/s^2 加速度单位
 * 和 °/s 
 * 但是会丢失大量精度
*/
void parse_6_axises_data(mpu6050_data * raw_data,accleration * acc,angular_velocity * ang_v){

    // 最大的两字节有符号整数，即 32767
    const int16_t max_2_bytes_int = 0x7fff;

    // 16383
    const float half_of_max_2_bytes_int = max_2_bytes_int>>1;

    // 因为上面 mpu6050 配置的 加速度计最大量程为 +-2g 
    // 2g 对应 32767
    // 1g 对应 16383
    // x轴加速度 = (x_acc /32767) * 2
    // 化简之后就为这里的 xacc / 16383
    acc->ax = raw_data->x_acc/half_of_max_2_bytes_int;
    acc->ay = raw_data->y_acc/half_of_max_2_bytes_int;
    acc->az = raw_data->z_acc/half_of_max_2_bytes_int;

    //因为上面配置的角速度传感器的量程为 +- 250°/s
    // 比例系数 = (x_gry / 32767) * 250
    const float gyr_proportional_coefficient = 0.0076;
    ang_v->gx = raw_data->x_gyr * gyr_proportional_coefficient;
    ang_v->gy = raw_data->y_gyr * gyr_proportional_coefficient;
    ang_v->gz = raw_data->z_gyr * gyr_proportional_coefficient;
}


void parse_euler_angles(accleration * acc,angular_velocity * ang_v){
    
    // 使用加速度计
    
    euler_angles.roll_acc = atan2(acc->ay,acc->az);
    
    // 因为定义中俯仰角的方向与y轴旋转方向正好相反，所以要加一个负号
    euler_angles.pitch_acc = -atan2(acc->az,acc->ay);
    

    
    euler_angles.roll_gyr += ang_v->gx * dt;
    euler_angles.pitch_gyr += ang_v->gy * dt;
    euler_angles.yaw += ang_v->gz * dt;
}

/**
分别计算加速度计和陀螺仪的欧拉角
*/
void calculate_euler_angles(Integrated_Euler_Angles * integrated_euler_angles){

    // 从 mpu6050 中读取6轴经过ADC转换的原始数据
    mpu6050_data * raw_data;
    raw_data = read_6_axises();
    
    
    // 把原始数据转换为标准的物理单位
    accleration  acc; // m/s^2
    angular_velocity  ang_v;  // °/s
    parse_6_axises_data( raw_data,&acc,&ang_v);
    
    
    parse_euler_angles(&acc,&ang_v);
    
    // 加速度计无法得出偏航角，所以直接使用陀螺仪的偏航角
    integrated_euler_angles->yaw = euler_angles.yaw;
    
    const static float alpha = 0;//0.1/(0.1*dt); // 0.95238
    integrated_euler_angles->roll = (1-alpha)*euler_angles.roll_acc + alpha * euler_angles.roll_gyr;
    integrated_euler_angles->pitch = (1-alpha)*euler_angles.pitch_acc + alpha * euler_angles.pitch_gyr;

}
